/**
 * 监听 /pose2D → 发布 /scanmatcher_odom
 */
#include <geometry_msgs/Pose2D.h>
#include <nav_msgs/Odometry.h>
#include <ros/ros.h>
#include <tf2/LinearMath/Quaternion.h>

ros::Publisher odom_pub;

void pose2DCallback(const geometry_msgs::Pose2D::ConstPtr& msg)
{
    nav_msgs::Odometry odom_msg;
    odom_msg.header.stamp = ros::Time::now();
    odom_msg.header.frame_id = "odom";
    odom_msg.child_frame_id = "base_link";

    // 将 Pose2D 的数据填入 Odometry 消息
    odom_msg.pose.pose.position.x = msg->x;
    odom_msg.pose.pose.position.y = msg->y;
    odom_msg.pose.pose.position.z = 0.0;

    // 将 yaw 转为四元数
    tf2::Quaternion q;
    q.setRPY(0, 0, msg->theta);
    odom_msg.pose.pose.orientation.x = q.x();
    odom_msg.pose.pose.orientation.y = q.y();
    odom_msg.pose.pose.orientation.z = q.z();
    odom_msg.pose.pose.orientation.w = q.w();

    // 暂时不填速度
    odom_msg.twist.twist.linear.x = 0.0;
    odom_msg.twist.twist.linear.y = 0.0;
    odom_msg.twist.twist.angular.z = 0.0;

    odom_pub.publish(odom_msg);
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "pose2d_to_odom");
    ros::NodeHandle nh;

    odom_pub = nh.advertise<nav_msgs::Odometry>("/scanmatcher_odom", 10);
    ros::Subscriber pose2d_sub = nh.subscribe("/pose2D", 10, pose2DCallback);

    ros::spin();
    return 0;
}